#include "first_person_camera.h"


FirstPersonCamera::FirstPersonCamera(Point at,Point eye,Point up,float fovUp,float aspect,float dNear,float dFar)
	: PerspectiveCamera(eye,at,up,fovUp,aspect,dNear,dFar){
}

FirstPersonCamera::~FirstPersonCamera(){

}

void FirstPersonCamera::applyModelViewTransformations(){
	glMatrixMode(GL_MODELVIEW);
	glLoadIdentity();

	Point eye = this->getEye();
	Point at = this->getAt();
	Point up = this->getUp();
	gluLookAt(eye.x1, eye.x2, eye.x3, at.x1, at.x2, at.x3, up.x1, up.x2, up.x3);
}

void FirstPersonCamera::rotateUD(float theta){
	Point eye = this->getEye();
	Point dir = this->getAt()-this->getEye();
	float xytheta = -atan2(dir.x1,dir.x2);
	float yztheta = atan2(dir.x3,sqrt(pow(dir.x1,2)+pow(dir.x2,2)));

	if(fabs(yztheta+theta)>=PI/2)
		return;

	Matrix4x4 t1(-eye.x1,-eye.x2,-eye.x3);
	Matrix4x4 r1(-xytheta,Point(0,0,1));
	Matrix4x4 r2(theta,Point(1,0,0));
	Matrix4x4 r3(xytheta,Point(0,0,1));
	Matrix4x4 t2(eye.x1,eye.x2,eye.x3);

	Matrix4x4 t;
	t *= t1;
	t *= r1;
	t *= r2;
	t *= r3;
	t *= t2;

	frustum.applyTransformation(t);
}

void FirstPersonCamera::moveTo(const Point& p){
	Point diff = p - this->getEye();
	Matrix4x4 t1(diff.x1,diff.x2,diff.x3);
	frustum.applyTransformation(t1);
}

void FirstPersonCamera::move(float offset){
	// TODO
	throw std::string("no implementado");
}

// No se considera el z
void FirstPersonCamera::moveAhead(float offset){
	Point aheadDir = (this->getEye()-this->getAt());
	aheadDir.x3 = 0;

	Point move = aheadDir*offset;
	Matrix4x4 m(move.x1,move.x2,move.x3);
	frustum.applyTransformation(m);
}

void FirstPersonCamera::strafe(float offset){
	Point strafeDir = (this->getEye()-this->getAt()).cross(this->getUp());
	Point move = strafeDir*offset;
	Matrix4x4 m(move.x1,move.x2,move.x3);
	frustum.applyTransformation(m);
}

void FirstPersonCamera::rotateLR(float theta){
	Point eye = this->getEye();
	Matrix4x4 t1(-eye.x1,-eye.x2,-eye.x3);
	Matrix4x4 r1(theta,Point(0,0,1));
	Matrix4x4 t2(eye.x1,eye.x2,eye.x3);

	Matrix4x4 t;
	t *= t1;
	t *= r1;
	t *= t2;

	frustum.applyTransformation(t);
}

void FirstPersonCamera::updatePosition(const Point& position,const Point& direction,PathInfo* info){
	// mal!!!!...el up podria no ser normal al vector direccion(cuando se esta mirando para arriba o abajo)
	frustum.updateFrustum(position,position+direction,frustum.getFrustumUp(),_fovUp,_aspect,_dNear,_dFar);
}
